//============================================================================
// Name        : AutonomeSystemeOpenCV.cpp
// Author      : Ruben Schwarz
// Version     :
// Copyright   : Your copyright notice
// Description : Hello World in C++, Ansi-style
//============================================================================

#include <iostream>
#include <stdio.h>
#include <string.h>
#include <highgui.h>
#include <cxcore.h>
#include <cv.h>
#include "tracker.h"
#include <ctime>

#define BLAU 0
#define GRUEN 1
#define ROT 2

#define HUE 0
#define SAT 1
#define VAL 2

#define C_B	2
#define C_R 0
#define Y	1

#define WAIT 2000

using namespace std;
using namespace cv;

//use the webcam instead of testpics
//#define LIVECAM
#define TESTDATEN1
//#define WRITE_TO_DISK

int main() {

	Mat frame;

	int resultCounter = 0;
	char resultName[20];
#ifdef WRITE_TO_DISK
	char saveName[20];
#endif

#ifndef LIVECAM
	int bildnr = 0;
	char sfilename[512];
#endif

#ifdef LIVECAM
	//VideoCapture camera(0);
	VideoCapture camera(0);

	if (!camera.isOpened())
		return -1;
#endif


	clock_t c1,c2,cd, cavg,ct1,ct2,ctd;
	cavg = clock();

  while( 1 )
  	{
#ifdef LIVECAM
  	cvWaitKey(20);
		camera >> frame; // get a new frame from camera
#else
		ct1 = clock();
		cvWaitKey(WAIT);
		ct2 = clock();
		if(bildnr >=
#ifdef TESTDATEN1
				37
#else
				21
#endif
				)
			break;
		// neues Bild einlesen
		bildnr++;

		c1 = clock();

#ifdef TESTDATEN1
		sprintf(sfilename, "C:/Users/nick/workspace/AutonomeSystemeOpenCV/src/img/2011-06-07-140641_%d.jpg", bildnr); //38
#else
		sprintf(sfilename, "C:/Users/nick/workspace/AutonomeSystemeOpenCV/src/img/2011-06-07-140840_%d.jpg", bildnr); //21
#endif
		//sprintf(sfilename, "/home/ruben/Dokumente/aalen/2_semester/AutonomeSysteme/testbilderGross/2011-06-07-140504_%d.jpg", bildnr);
		//sprintf(sfilename, "/home/ruben/Dokumente/aalen/2_semester/AutonomeSysteme/testbilderGross/2011-06-07-140641_%d.jpg", bildnr);
		//sprintf(sfilename, "C:/Users/nick/workspace/AutonomeSystemeOpenCV/src/img/2011-06-07-140641_%d.jpg", bildnr); //38
		//sprintf(sfilename, "img/2011-06-07-140840_%d.jpg", bildnr); //21
		//sprintf(sfilename, "/home/ruben/Dokumente/aalen/2_semester/AutonomeSysteme/testbilderGross/2011-06-07-140840_%d.jpg", bildnr);

		// Bild laden
		frame = cvLoadImage( sfilename, CV_LOAD_IMAGE_COLOR);
		//frame = cvLoadImage( "C:/Users/nick/workspace/AutonomeSystemeOpenCV/src/img/ehmNix.png" , CV_LOAD_IMAGE_COLOR);
#endif


		// Bild in den Tracker stecken - falls er true zurueck gibt wurde begonnen eine markierung zu tracken.
		if(CTracker::instance()->processFrame(frame))
		{
#if 1
			//gefundene markierung anzeigen
			++resultCounter;
			sprintf(resultName, "RESULT_%d", resultCounter);
			namedWindow(resultName, 1);
			cvMoveWindow(resultName, 680, 200);
			Mat *res;
			res = CTracker::instance()->getTrackedMark();
			imshow(resultName, *res );
#endif
		}
		
		c2 = clock();

		cd = c2 - c1;
		stringstream str;
		//str << td;

		cavg = 0.8*cavg + 0.2*cd;
		ctd = ct2 - ct1;

		str << cd;
		str << " ms     AVG:";
		str << cavg;
		str << " CLOCKTEST( ";
		str << WAIT;
		str << "):";
		str << ctd;


		Mat out = CTracker::instance()->latestProcessedFrame();
		CTracker::writeStringToFrame(out,	str.str() );
		namedWindow("Output", 1);
		cvMoveWindow("Output", 0, 200);
		imshow("Output", out );

#ifdef WRITE_TO_DISK
		IplImage img = CTracker::instance()->latestProcessedFrame().operator _IplImage();
		IplImage *pImg = &img;
		sprintf(saveName, "out_%d.jpg", bildnr); //38
		cvSaveImage( saveName ,pImg );
#endif

			/*
		 * Verarbeitung des BGR-Bildes
		 */

		/*
		Mat mask;
		vector<Vec3f> circles;
		Mat output, outYC;
		vector<Mat> RGBcolors;
		vector<Mat> HSVcolors;
		vector<Mat> YCcolors;
		*/

		/*
		 split(frame, RGBcolors);

		 threshold(RGBcolors[ROT], RGBcolors[ROT], 180, 255, CV_THRESH_BINARY);
		 // Werte Blau-Anteil aus
		 threshold(RGBcolors[BLAU], mask, 100, 100, CV_THRESH_BINARY);
		 RGBcolors[ROT].setTo(0, mask);
		 // Werte GRUEN-Anteil aus
		 threshold(RGBcolors[GRUEN], mask, 100, 100, CV_THRESH_BINARY);
		 RGBcolors[ROT].setTo(0, mask);

		 HoughCircles(RGBcolors[ROT], circles, CV_HOUGH_GRADIENT, 2,
		 RGBcolors[ROT].cols / 2, 100, 100, 20);

		 for (size_t i = 0; i < circles.size(); i++) {
		 Point center(cvRound(circles[i][0]), cvRound(circles[i][1]));
		 int radius = cvRound(circles[i][2]);
		 // draw the circle center
		 circle(RGBcolors[ROT], center, 3, Scalar(255, 255, 255), -1, 8, 0);
		 // draw the circle outline
		 circle(RGBcolors[ROT], center, radius, 100, 3, 8, 0);
		 }


		 namedWindow("Rot-Anteil", 1);
		 namedWindow("Grün-Anteil", 1);
		 namedWindow("Blau-Anteil", 1);
		 namedWindow("color", 1);
		 cvMoveWindow("Rot-Anteil", 0, 0);
		 cvMoveWindow("Grün-Anteil", 600, 0);
		 cvMoveWindow("Blau-Anteil", 0, 400);
		 cvMoveWindow("color", 600, 400);
		 imshow("Rot-Anteil", RGBcolors[ROT]);
		 imshow("Grün-Anteil", RGBcolors[GRUEN]);
		 imshow("Blau-Anteil", RGBcolors[BLAU]);
		 imshow("color", frame);
		 */

		/*
		 * Verarbeitung im HSV-Farbraum
		 */
		/*
		 cvtColor(frame, output, CV_BGR2HSV);
		 split(output, HSVcolors);

		 threshold(hsvColors[SAT], mask, 150, 255, CV_THRESH_BINARY);

		 //hsvColors[HUE].setTo(255, mask);

		 namedWindow("HUE-Anteil", 1);
		 namedWindow("SAT-Anteil", 1);
		 namedWindow("VAL-Anteil", 1);
		 namedWindow("color", 1);
		 cvMoveWindow("HUE-Anteil", 0, 0);
		 cvMoveWindow("SAT-Anteil", 600, 0);
		 cvMoveWindow("VAL-Anteil", 0, 400);
		 cvMoveWindow("color", 600, 400);
		 imshow("HUE-Anteil", hsvColors[HUE]);
		 imshow("SAT-Anteil", hsvColors[SAT]);
		 imshow("VAL-Anteil", hsvColors[VAL]);
		 imshow("color", frame);
		 */

		/*
		 * Verarbeitung im YCrCb-Farbraum
		 */
/*
		cvtColor(frame, outYC, CV_BGR2YCrCb);
		split(outYC, YCcolors);

		threshold(YCcolors[Y], YCcolors[Y], 170, 255, CV_THRESH_BINARY);

		HoughCircles(YCcolors[Y], circles, CV_HOUGH_GRADIENT, 2,
				YCcolors[Y].cols / 4);

		for (size_t i = 0; i < circles.size(); i++) {
			Point center(cvRound(circles[i][0]), cvRound(circles[i][1]));
			int radius = cvRound(circles[i][2]);
			// draw the circle center
			circle(YCcolors[Y], center, 3, Scalar(100, 100, 100), -1, 8, 0);
			// draw the circle outline
			circle(YCcolors[Y], center, radius, 100, 3, 8, 0);
		}

		namedWindow("Cr-Anteil", 1);
		namedWindow("Cb-Anteil", 1);
		namedWindow("Y-Anteil", 1);
		namedWindow("color", 1);
		cvMoveWindow("Cr-Anteil", 0, 0);
		cvMoveWindow("Cb-Anteil", 600, 0);
		cvMoveWindow("Y-Anteil", 0, 400);
		cvMoveWindow("color", 600, 200);
		imshow("Cr-Anteil", YCcolors[C_R]);
		imshow("Y-Anteil", YCcolors[Y]);
		imshow("Cb-Anteil", YCcolors[C_B]);
		imshow("color", frame);
*/		
	}
	// the camera will be deinitialized automatically in VideoCapture destructor

	return 0;
}

